#pragma once
#include <iostream>
#include <assert.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace MathLibrary
{
/*
	template<typename T>
	class Vector2
	{
	public:
		Vector2(void):x(T(0)), y(T(0)){}
		~Vector2(void) {}
		Vector2(const T & _x,const T& _y):x(_x), y(_y){}
		Vector2(T * data){x = data[0]; y = data[1];}

		void set(T & _x, T& _y) {x = _x; y = _y;}
		
		Vector2 operator + (const Vector2 & v) const {return Vector2(x + v.x, y + v.y);}
		Vector2 operator - (const Vector2 & v) const {return Vector2(x - v.x, y - v.y);}
		Vector2 operator * (const T & c) const {return Vector2(c * x , c * y);}
		friend Vector2 operator * (const T & c, const Vector2 & v)  {return Vector2(c * v.x , c * v.y);}
		Vector2 operator / (const T & c) const {return Vector2( x / c , y / c);}
		friend Vector2 operator / (const T & c, const Vector2 & v) {return Vector2(c / v.x , c / v.y);}
		
		Vector2 & operator += (const Vector2 & v){x += v.x; y += v.y; return *this;}
		Vector2 & operator -= (const Vector2 & v) {x -= v.x; y -= v.y; return *this;}
		Vector2 & operator *= (const T & c) {x *= c; y *= c; return *this;}
		Vector2 & operator /= (const T & c) {x /= c; y /= c; return *this;}
		
		
		
		
		T dot(const Vector2 & v) const {return x * v.x + y * v.y;}
		T getMagnitude() const {return sqrt(x*x + y*y);}
		T getMagnitudeSquare() const {return x*x + y*y;}
		void normalize() {*this /= getMagnitude();}




		friend std::ostream & operator<<(std::ostream &os, const Vector2& v)
		{
			os << "[" << v.x << "; " << v.y << "]" << std::endl;
			return os;
		}

	public:
		T x;
		T y;
	};


	template<typename T>
	class Vector3
	{
	public:
		Vector3(void):x(T(0)), y(T(0)), z(T(0)){}
		~Vector3(void){}
		Vector3(const T & _x,const T& _y, const T & _z):x(_x), y(_y), z(_z){}
		Vector3(T * data){x = data[0]; y = data[1]; z = data[2];}

		void set(T & _x, T& _y, T & _z) {x = _x; y = _y; z = _z;}
		
		Vector3 operator + (const Vector3 & v) const {return Vector3(x + v.x, y + v.y, z + v.z);}
		Vector3 operator - (const Vector3 & v) const {return Vector3(x - v.x, y - v.y, z - v.z);}
		Vector3 operator * (const T & c) const {return Vector3(c * x , c * y, c * z);}
		friend Vector3 operator * (const T & c, const Vector3 & v)  
		{
			return Vector3(c * v.x , c * v.y, c * v.z);
		}
		Vector3 operator / (const T & c) const 
		{
			assert(abs(c) >= std::numeric_limits<T>::epsilon());
			return Vector3( x / c , y / c, z / c);
		}
		friend Vector3 operator / (const T & c, const Vector3 & v) 
		{
			return Vector3(c / v.x , c / v.y, c / v.z);
		}
		
		Vector3 & operator += (const Vector3 & v){x += v.x; y += v.y; z += v.z; return *this;}
		Vector3 & operator -= (const Vector3 & v) {x -= v.x; y -= v.y; z -= v.z; return *this;}
		Vector3 & operator *= (const T & c) 
		{
			x *= c; y *= c; z *= c; return *this;
		}
		Vector3 & operator /= (const T & c) 
		{
			assert(abs(c) >= std::numeric_limits<T>::epsilon());
			x /= c; y /= c; z /= c; return *this;
		}
		
		
		T dot(const Vector3 & v) const {return x*v.x + y*v.y + z*v.z;}
		T getMagnitude() const 
		{
			assert(abs(x) >= std::numeric_limits<T>::epsilon() ||
				abs(y) >= std::numeric_limits<T>::epsilon() ||
				abs(z) >= std::numeric_limits<T>::epsilon());
			return sqrt(x*x + y*y + z*z);
		}
		T getMagnitudeSquare() const {return x*x + y*y + z*z;}
		void normalize() {*this /= getMagnitude();}

		friend std::ostream & operator<<(std::ostream &os, const Vector3& v)
		{
			os << "[" << v.x << "; " << v.y << "; " << v.z << "]" << std::endl;
			return os;
		}

		T x;
		T y;
		T z;
	};

	template<typename T>
	class Vector4
	{
	public:
		Vector4(void):x(T(0)), y(T(0)), z(T(0)), w(T(1)){}
		Vector4(Vector3<T> v):x(v.x), y(v.y), z(v.z), w(T(1)){}

		~Vector4(void){}
		Vector4(const T & _x,const T& _y, const T & _z, const T & _w = 1):x(_x), y(_y), z(_z), w(_w){}
		Vector4(T * data){x = data[0]; y = data[1]; z = data[2]; w = data[3];}

		void set(T & _x, T& _y, T & _z, T & _w) {x = _x; y = _y; z = _z; w = _w};
		void set(T & _x, T& _y, T & _z) {x = _x; y = _y; z = _z; w = 1};
		
		void simplify() 
		{
			assert( abs(w) >= std::numeric_limits<float>::epsilon());
			x /= w; y /= w; z /= w; w = T(1);
		}

		Vector4 operator + (const Vector4 & v) const 
		{
			return Vector4(x/w + v.x/v.w, y / w + v.y / v.w, z / w + v.z / v.w);
		}
		Vector4 operator - (const Vector4 & v) const 
		{
			return Vector4(x/w - v.x/v.w, y/w - v.y/v.w, z/w - v.z/v.w);
		}
		Vector4 operator * (const T & c) const 
		{
			assert(abs(c) >= std::numeric_limits<T>::epsilon());
			return Vector4(c * x / w , c * y / w, c * z / w);
		}
		friend Vector4 operator * (const T & c, const Vector4 & v)
		{
			return Vector4(c * v.x / v.w , c * v.y / v.w, c * v.z / v.w);
		}
		Vector4 operator / (const T & c) 
		{
			simplify();
			assert(abs(c) >= std::numeric_limits<T>::epsilon());
			return Vector4( x / c , y / c, z / c);
		}
		friend Vector4 operator / (const T & c, const Vector4 & v) 
		{
			assert(abs(v.w) >= std::numeric_limits<T>::epsilon() && abs(c) >= std::numeric_limits<T>::epsilon());
			return Vector4(c / v.x , c / v.y, c / v.z, c / v.w);
		}
		
		Vector4 & operator += (const Vector4 & v)
		{
			assert(abs(v.w) >= std::numeric_limits<T>::epsilon());
			simplify();
			x += v.x / v.w; y += v.y / v.w; z += v.z / v.w; return *this;
		}
		Vector4 & operator -= (const Vector4 & v) 
		{
			assert(abs(v.w) >= std::numeric_limits<T>::epsilon());
			simplify();
			x -= v.x / v.w; y -= v.y / v.w; z -= v.z / v.w; 
			return *this;
		}
		Vector4 & operator *= (const T & c) 
		{
			simplify();
			x *= c; y *= c; z *= c; return *this;
		}
		Vector4 & operator /= (const T & c) 
		{
			assert(abs(c) >= std::numeric_limits<T>::epsilon());
			x /= c; y /= c; z /= c; return *this;
		}
		
		T dot(const Vector4 & v) const {simplify(); return x*v.x / v.w  + y*v.y / v.w + z*v.z / v.w;}
		T getMagnitude() 
		{
			assert(abs(x) >= std::numeric_limits<T>::epsilon() ||
				abs(y) >= std::numeric_limits<T>::epsilon() ||
				abs(z) >= std::numeric_limits<T>::epsilon());
			simplify();
			return sqrt(x*x + y*y + z*z);
		}
		T getMagnitudeSquare() const {simplify(); return x*x + y*y + z*z;}
		void normalize() {*this /= getMagnitude();}




		friend std::ostream & operator<<(std::ostream &os, const Vector4& v)
		{
			os << "[" << v.x << "; " << v.y << "; " << v.z << "; " << v.w << "]" << std::endl;
			return os;
		}

		T x;
		T y;
		T z;
		T w;
	};

	typedef Vector2<float> Vector2f;
	typedef Vector2<double> Vector2d;
	typedef Vector2<int> Vector2i;
	typedef Vector2<short> Vector2s;


	typedef Vector3<float> Vector3f;
	typedef Vector3<double> Vector3d;
	typedef Vector3<int> Vector3i;
	typedef Vector3<short> Vector3s;

	typedef Vector4<float> Vector4f;
	typedef Vector4<double> Vector4d;
	typedef Vector4<int> Vector4i;
	typedef Vector4<short> Vector4s;
	*/
	
	typedef Eigen::Matrix<float,2,1,Eigen::ColMajor,2,1> Vector2f;
	typedef Eigen::Matrix<double,2,1,Eigen::ColMajor,2,1> Vector2d;
	typedef Eigen::Matrix<int,2,1,Eigen::ColMajor,2,1> Vector2i;
	typedef Eigen::Matrix<short,2,1,Eigen::ColMajor,2,1> Vector2s;


	typedef Eigen::Matrix<float,3,1,Eigen::ColMajor,3,1> Vector3f;
	typedef Eigen::Matrix<double,3,1,Eigen::ColMajor,3,1> Vector3d;
	typedef Eigen::Matrix<int,3,1,Eigen::ColMajor,3,1> Vector3i;
	typedef Eigen::Matrix<short,3,1,Eigen::ColMajor,3,1> Vector3s;
	
	typedef Eigen::Matrix<float,4,1,Eigen::ColMajor,4,1> Vector4f;
	typedef Eigen::Matrix<double,4,1,Eigen::ColMajor,4,1> Vector4d;
	typedef Eigen::Matrix<int,4,1,Eigen::ColMajor,4,1> Vector4i;
	typedef Eigen::Matrix<short,4,1,Eigen::ColMajor,4,1> Vector4s;


	typedef Eigen::Matrix<float,2,2,Eigen::RowMajor,2,2> Matrix2f;
	typedef Eigen::Matrix<double,2,2,Eigen::RowMajor,2,2> Matrix2d;
	typedef Eigen::Matrix<int,2,2,Eigen::RowMajor,2,2> Matrix2i;
	typedef Eigen::Matrix<short,2,2,Eigen::RowMajor,2,2> Matrix2s;

	typedef Eigen::Matrix<float,3,3,Eigen::RowMajor,3,3> Matrix3f;
	typedef Eigen::Matrix<double,3,3,Eigen::RowMajor,3,3> Matrix3d;
	typedef Eigen::Matrix<int,3,3,Eigen::RowMajor,3,3> Matrix3i;
	typedef Eigen::Matrix<short,3,3,Eigen::RowMajor,3,3> Matrix3s;

	typedef Eigen::Matrix<float,4,4,Eigen::RowMajor,4,4> Matrix4f;
	typedef Eigen::Matrix<double,4,4,Eigen::RowMajor,4,4> Matrix4d;
	typedef Eigen::Matrix<int,4,4,Eigen::RowMajor,4,4> Matrix4i;
	typedef Eigen::Matrix<short,4,4,Eigen::RowMajor,4,4> Matrix4s;


	/*
	typedef Vector3<float> Vector3f;
	typedef Vector3<double> Vector3d;
	typedef Vector3<int> Vector3i;
	typedef Vector3<short> Vector3s;

	typedef Vector4<float> Vector4f;
	typedef Vector4<double> Vector4d;
	typedef Vector4<int> Vector4i;
	typedef Vector4<short> Vector4s;
	*/
}
